#include "starrobot_bringup.h"
using namespace std;
starrobot_bringup::starrobot_bringup():
	baud_data(115200),RecLength(0)
{
	// Get Luncher file define value 
	ros::NodeHandle nh_private("~");
	nh_private.param<std::string>("port", this->usart_port, "/dev/starrobotbase"); 
 	nh_private.param<int>("baud", this->baud_data, 115200); 
 	ROS_INFO("Port:%s",usart_port.c_str());
	/**open seril device**/
	try{
		 Robot_Serial.setPort(this->usart_port);
		 Robot_Serial.setBaudrate(this->baud_data);
		 serial::Timeout to = serial::Timeout::simpleTimeout(200);
		 Robot_Serial.setTimeout(to);
		 Robot_Serial.open();
  	}
	catch (serial::IOException& e){
		 ROS_ERROR_STREAM("[starrobot] Unable to open serial port, please check device or permission");
	}
	if(Robot_Serial.isOpen())
	{
	 	 ROS_INFO_STREAM("[starrobot] Successful opening of the serial port, data transmission started");
	}
	else
	{
 		ROS_ERROR_STREAM("[starrobot] Unable to open serial port, please check device or permission");
	}
	memset(RecBuff,0,512);
}
starrobot_bringup::~starrobot_bringup()
{
	Robot_Serial.close();
}
/***
 @ Description	-> send and get stm32 board data
 @ Param		-> null
 @ Author		-> ChenZhouFu
 @ Date			-> 2019-03-10
 @ Function     -> bool starrobot_bringup::ReadFormUart()
 @ return 		-> status
***/
void starrobot_bringup::ReadFormUart(void)
{	
	int i ;
	while(ros::ok())
	{
		RecLength = Robot_Serial.read(RecBuff,512);
		if(RecLength !=0)
		{
			printf("recLength:%d Date:",RecLength);
			for(i = 0;i<RecLength;i++)
			{
				printf(" %02x ",RecBuff[i]);
			}
			printf("\n\r");
			memset(RecBuff,0,512);
		}
	}
}


